#include "web_command.hpp"

#include "httplib.hpp"
#include <iostream>
#include <string>
#include <move_base_msgs/MoveBaseAction.h>  
#include "tool.hpp"
#include <jsoncpp/json/json.h>
#include <thread> 

using namespace std;

struct target_point
{
    double x;
    double y;
    double yaw;
};


target_point * param( string body){
    Json::Reader reader;
    Json::Value root;
    if (!reader.parse(body, root)) {
        cout<<"parse error"<<endl;
        return nullptr;
    } 
    target_point * tp = new target_point;
    tp->x = root["x"].asDouble();
    tp->y = root["y"].asDouble();
    tp->yaw = root["yaw"].asDouble();
    return tp;
}

void WebCommand::run_http(int port)
 {
    cout<<"web command start at port:"<<port<<endl;
    httplib::Server svr;
 
    svr.Get("/", [](const httplib::Request& req, httplib::Response& res) {
        res.set_content("hello,world", "text/plain");
    });

    svr.Post("/moveto", [&](const httplib::Request& req, httplib::Response& res) {
        string body = req.body;
        cout<<"body:"<<body<<endl;
        target_point * tp = param(body);
        if(tp == nullptr)
        {
            res.set_content("error", "text/plain");
            return;
        }else{
            this->moveTo(tp->x,tp->y,tp->yaw); 
            delete tp;
            res.set_content("success", "text/plain");
       } 
    });

    svr.listen("0.0.0.0", port);
    while (running)
    { 
        this_thread::sleep_for(chrono::seconds(1));
    }
    
 }


void WebCommand::start(int port)
{
    if(running) return;
    running=true;
    thread t(&WebCommand::run_http,this,port);
    t.detach(); 
}

WebCommand::WebCommand( ros::NodeHandle &handle)
{
  
    publish_ = handle.advertise<std_msgs::String>("web_command", 1000);
}

WebCommand::~WebCommand()
{
    running=false;
}


/// @brief 移动到目标处
/// @param x 
/// @param y 
/// @param yaw 
void WebCommand::moveTo(double x,double y,double yaw)
{
    cout<<"move to x:"<<x<<" y:"<<y<<" yaw:"<<yaw<<endl;

    move_base_msgs::MoveBaseGoal goal;  
    goal.target_pose.header.frame_id = "map"; // 设置参考坐标系，通常是map  
    goal.target_pose.header.stamp = ros::Time::now();  
    goal.target_pose.pose.position.x = x;
    goal.target_pose.pose.position.y = y;
    goal.target_pose.pose.position.z = 0;

    quaternion * q = yaw2quaternion(yaw); 
    goal.target_pose.pose.orientation.w =q->w;
    goal.target_pose.pose.orientation.x =q->x;
    goal.target_pose.pose.orientation.y =q->y;
    goal.target_pose.pose.orientation.z =q->z;

    delete q;

    publish_.publish(goal);
}

 